﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace AstarTest2
{   // Pseudocode: http://www.scribd.com/doc/88243431/A-Star-Pseudo-Code
    class Map 
    {
        public int Rows;
        public int Columns;
        public Tile[,] Tiles;
        private int[,] tilemapping;

        private List<Tile> open = new List<Tile>();
        private List<Tile> closed = new List<Tile>();
        private List<Tile> path = new List<Tile>();

        private Tile start;
        private Tile goal;

        public Map()
        {
            tilemapping = new int[20, 20] { 
            //        ---> columns -->
            { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // r
            { 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0 }, // o
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // w
            { 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0 }, // s
            { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            { 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0 }, 
            { 0, 0, 0, 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 },
            { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // r
            { 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0 }, // o
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // w
            { 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0 }, // s
            { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            { 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0 }, 
            { 0, 0, 0, 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }
            };

            this.Rows = this.tilemapping.GetLength(0);
            this.Columns = this.tilemapping.GetLength(1);
            Tiles = new Tile[Rows, Columns];

            // Generate the tiles
            for (int row = 0; row < this.Rows; row++)
            {
                for (int column = 0; column < this.Columns; column++)
                {
                    this.Tiles[row, column] = new Tile(column, row, this.tilemapping[row, column], this);
                    Console.Write(this.tilemapping[row, column]);
                }
                Console.WriteLine();
            }

            // Generate adjacencies
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    Tiles[i, j].CalculateAdjacencies();
                }
            }
        }

        /// <summary>
        /// Execute the A-star algorithm.
        /// </summary>
        public void Astar()
        {
            start = Tiles[0, 0];
            // goal = Tiles[9, 17];
            goal = Tiles[10, 17];
            start.IsStart = true;
            goal.IsGoal = true;
            open.Add(start); // Add start tile to the open list initially

            while (open.Count > 0) // While open is not empty
            {
                Tile lowestCost = FindLowestCost();
                if (lowestCost == goal)
                {
                    ReconstructPath(goal);
                    Print();
                    return;
                }
                else // Goal hasn't been found yet
                {
                    open.Remove(lowestCost);
                    closed.Add(lowestCost);
                    foreach (Tile neighbor in lowestCost.Adjacencies)
                    {
                        if (closed.Contains(neighbor))
                            continue;

                        // Get the cost of the path until the best tile, and the cost of the estimated distance to the 
                        // neighbor tile added together
                        int tentativeParentCost = lowestCost.GetParentCost() + lowestCost.GetHeuristicCost(neighbor);
                        bool tentativeIsBetter = false;

                        if (!open.Contains(neighbor))
                        {
                            open.Add(neighbor);
                            neighbor.HeuristicCost = neighbor.GetHeuristicCost(goal);
                            tentativeIsBetter = true;
                        }
                        // Predicted tentative parent cost is smaller than the actual parent cost of the neighbor
                        else if (tentativeParentCost < neighbor.GetParentCost())
                            tentativeIsBetter = true;
                        else
                            tentativeIsBetter = false;

                        if (tentativeIsBetter)
                        {
                            neighbor.Parent = lowestCost;
                            neighbor.ParentCost = tentativeParentCost;
                            neighbor.EstimatedTotalCost = neighbor.ParentCost + neighbor.HeuristicCost;
                            // Note: Heuristic cost of neighbor has always been set at this stage
                        }
                    }
                }
            }
        }

        /// <summary>
        /// Find the tile via which the path (from start to goal) is the shortest.
        /// </summary>
        /// <returns></returns>
        private Tile FindLowestCost()
        {
            Tile best = null;
            foreach (Tile tile in open)
            {
                if (best == null || tile.GetTotalCost(goal) < best.GetTotalCost(goal))
                {
                    best = tile;
                }
            }

            return best;
        }

        /// <summary>
        /// Reconstructs the path, filling the path from the tile after the start tile to the goal tile.
        /// </summary>
        /// <param name="tile">The tile to recursively reconstruct from (the goal).</param>
        private void ReconstructPath(Tile tile)
        {
            // came_from = path
            path.Add(tile);
            if (tile.Parent.IsStart == false)
            {
                ReconstructPath(tile.Parent);
            }
        }

        /// <summary>
        /// Prints the map and the calculated path.
        /// </summary>
        private void Print()
        {
            Console.WriteLine("Solution: \n----------");
            for (int row = 0; row < this.Rows; row++)
            {
                for (int column = 0; column < this.Columns; column++)
                {
                    Tile tile = this.Tiles[row, column];
                    if (tile.IsStart)
                        Console.Write("S");
                    else if (tile.IsGoal)
                        Console.Write("E");
                    else if (this.path.Contains(tile))
                        Console.Write(".");
                    else
                        Console.Write(tile.Type);
                }
                Console.WriteLine();
            }
            Console.WriteLine("----------");
        }
    }
}
